module mymodule

use constants
use select_precision
use dispmodule
implicit none

    type conductor
        character(len=32)                   :: name
        integer                             :: ID
        real(wp)                            :: sigma
        integer                             :: Nsegments      
        real(wp),dimension(:,:),allocatable :: segments
        integer                             :: Nvertices
        real(wp),dimension(:,:),allocatable :: vertices
    end type conductor


contains

    
    

    subroutine linspace( array , xstart , xstop  )
        implicit none
        real(kind=wp),intent(in)                 :: xstart,xstop
        real(kind=wp),intent(inout),dimension(:) :: array
        integer                                  :: i,N
        
        N = size( array ) 
        
        forall( i=1:N ) array(i) = (xstop-xstart) * real(i-1,wp) / real(N-1,wp) + xstart
        
        return
    end subroutine linspace
    
    pure function mymidpoint( vstart , vstop )
    !Calculates the midpoint between two vertices of the form
    ! vstart = [ x_start , y_start ]
        implicit none
        real(kind=wp),dimension(2),intent(in)    :: vstart,vstop
        real(kind=wp),dimension(2)               :: mymidpoint        
        
        mymidpoint(1) = vstart(1) + HALF*(vstop(1) - vstart(1) )
        mymidpoint(2) = vstart(2) + HALF*(vstop(2) - vstart(2) )
    end function mymidpoint
    
    pure function length( vstart , vstop )
    !Calculates the length between two vertices of the form
    ! vstart = [ x_start , y_start ]
        implicit none
        real(kind=wp),dimension(2),intent(in)    :: vstart,vstop
        real(kind=wp)                            :: length
        
        length = sqrt( (vstart(1)-vstop(1))**2 + (vstart(2)-vstop(2))**2  )
    end function length
    
    type(conductor) function CalculateVertices( cond , stepsize  ) result(C)
        implicit none
        type( conductor ), intent(in)   :: cond
        real(wp),intent(in),optional    :: stepsize
        real(wp)                        :: mystep , tmplength
        real(wp),dimension(:)           :: tmpvertices
        integer,dimension(:)            :: Nsubvertices , i
        
        !Default to 10 segments linear dimension
        if (present(stepsize)) then
            mystep = stepsize
        else
            mystep = 0.1_wp
        endif
        
        allocate( Nsubvertices( cond%Nsegments ) )
        do i=1,cond%Nsegments !For all segments in cond
            tmplength       = length( cond%Nsegments(i,1:2) , cond%Nsegments(i,3:4 ) )
            Nsubvertices(i) = ceil( tmplength/mystep )
            allocate( tmpvertices(Nsubvertices(i)) )
            call linspace( tmpvertices , cond%Nsegments(i,1) , cond%Nsegments(i,3) )
            C%Nvertices     = 
            call linspace( tmpvertices , cond%Nsegments(i,2) , cond%Nsegments(i,4) )
            C%Nvertices     = 
        
        
        
    end function CalculateVertices    
        
    
    
    function CalculateImpedance( inner , outer , epsilon_r ) 
    !Calculates the impedance between two conductors specified by the 
        implicit none
        external dgesv
    
        type( conductor ),intent(in)                   :: inner , outer
        real(wp),intent(in)     :: epsilon_r
        real(wp)                :: CalculateImpedance
        integer                 :: Ntotal
    
        Ntotal = inner%Nvertices + outer%Nvertices - 2
    
    end function CalculateImpedance
    
    
    
    
    

  function impedance(a,b,c,d,p,s,alpha,epsilon_r,delta)
  
  implicit none
  external dgesv

  real(wp),intent(in)                       :: a,b,c,d,alpha,p,s,delta,epsilon_r
  real(wp),dimension(4)                     :: impedance
  real(wp)                                  :: inner_charge
  integer(ip)                               :: N1,N2,N3,N4,Ni,No,n,m,info,Ntot
  integer(ip),dimension(:),allocatable      :: ipiv
  real(wp),dimension(2,2)                   :: rot_matrix
  real(wp),dimension(2,1)                   :: center, midpoint
  real(wp),dimension(:,:),allocatable       :: inner_conductor, outer_conductor 
  real(wp),dimension(:,:),allocatable,target:: E
  real(wp),dimension(:,:),pointer           :: pAmat , pBmat , pCmat , pDmat , pu
  real(wp),dimension(:),allocatable         :: charge
  real(wp)                                  :: t_fill_start, t_fill_end , t_solve_start , t_solve_end
  logical,parameter                         :: DEBUG_PRINT = .FALSE. , USE_LAPACK = .TRUE.

  call cpu_time(t_fill_start)
  
  N1 = ceiling( a/delta ) 
  N2 = ceiling( b/delta ) 
  N3 = ceiling( c/delta ) 
  N4 = ceiling( d/delta )
  
  Ni = 2*(N1+N2)
  No = 2*(N3+N4)
  Ntot = Ni + No + 1
  
  
  rot_matrix(1,1) =  cos(alpha)
  rot_matrix(1,2) = -sin(alpha)
  rot_matrix(2,1) =  sin(alpha)
  rot_matrix(2,2) =  cos(alpha)
  
  center(1,1) = p
  center(2,1) = s
  
  allocate( inner_conductor(3,Ni) ,outer_conductor(3,No) )
  allocate( E(Ntot,Ntot+1_ip) ) !Solution vector will be present in the last column of E
  pAmat => E(    1:Ni       ,    1:Ni         )
  pBmat => E(    1:Ni       , Ni+1:Ni+No      )
  pCmat => E( Ni+1:Ni+No    ,    1:Ni         ) 
  pDmat => E( Ni+1:Ni+No    , Ni+1:Ni+No      ) 
  pu    => E(    1:Ni+No+1  , Ni+No+2:Ni+No+2 )
  
  !allocate( Amat(Ni,Ni) ,Bmat(Ni,No) , Cmat(No,Ni) , Dmat(No,No) )
  allocate( charge(Ni+No+1_ip) )

  
  do n=1,N1
    midpoint(1,1) = HALF*b
    midpoint(2,1) = a*( -HALF + (TWO*n-ONE)/(2*N1) )
    inner_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    inner_conductor(3,n) = QUARTER*a/N1
  enddo
  do n=N1+1,N1+N2
    midpoint(1,1) = b*( HALF - (TWO*(n-N1)-ONE)/(2*N2) )
    midpoint(2,1) = HALF*a
    inner_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    inner_conductor(3,n) = QUARTER*b/N2
  enddo
  do n=N1+N2+1,2*N1+N2
    midpoint(1,1) = -HALF*b
    midpoint(2,1) = a*( HALF - (TWO*(n-N1-N2)-ONE)/(2*N1) )
    inner_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    inner_conductor(3,n) = QUARTER*a/N1
  enddo
  do n=2*N1+N2+1,2*(N1+N2)
    midpoint(1,1) = b*( -HALF + (TWO*(n-2*N1-N2)-ONE)/(2*N2) )
    midpoint(2,1) = -HALF*a
    inner_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    inner_conductor(3,n) = QUARTER*b/N2
  enddo
    
  do n=1,N3
    midpoint(1,1) = HALF*d
    midpoint(2,1) = c*( -HALF + (TWO*(n)-ONE)/(2*N3) )
    outer_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    outer_conductor(3,n) = QUARTER*c/N3
  enddo
  do n=N3+1,N3+N4
    midpoint(1,1) = d*( HALF - (TWO*(n-N3)-ONE)/(2*N4) )
    midpoint(2,1) = HALF*c
    outer_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    outer_conductor(3,n) = QUARTER*d/N4
  enddo
  do n=N3+N4+1,2*N3+N4
    midpoint(1,1) = -HALF*d
    midpoint(2,1) = c*( HALF - (TWO*(n-N3-N4)-ONE)/(2*N3) )
    outer_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    outer_conductor(3,n) = QUARTER*c/N3
  enddo  
  do n=2*N3+N4+1,2*(N3+N4)
    midpoint(1,1) = d*( -HALF + (TWO*(n-2*N3-N4)-ONE)/(2*N4) )
    midpoint(2,1) = -HALF*c
    outer_conductor(1:2,n:n) = center + matmul( rot_matrix , midpoint )
    outer_conductor(3,n) = QUARTER*d/N4
  enddo  
  
  !Assemble voltage vector
  pu( 1:Ni , 1 ) = -FOURPI*epsilon_r*EPSILON0
  pu( Ni+1:Ni+No+1 , 1 ) = ZERO
  
  !Assemble stiffness matrix
  forall ( n=1:Ni , m=1:Ni , n==m ) pAmat(n,m) = log( inner_conductor( 3,n )**2 )
  forall ( n=1:Ni , m=1:Ni , n/=m ) pAmat(n,m) = log( norm2squared( inner_conductor(1:2,n) , inner_conductor(1:2,m) ) )

  forall ( n=1:No , m=1:No , n==m ) pDmat(n,m) = log( outer_conductor( 3,n )**2 )
  forall ( n=1:No , m=1:No , n/=m ) pDmat(n,m) = log( norm2squared( outer_conductor(1:2,n) , outer_conductor(1:2,m) ) )
  
  forall ( n=1:Ni , m=1:No )        pBmat(n,m) = log( norm2squared( inner_conductor(1:2,n) , outer_conductor(1:2,m) ) )
  
  forall ( n=1:No , m=1:Ni )        pCmat(n,m) = log( norm2squared( outer_conductor(1:2,n) , inner_conductor(1:2,m) ) )
              
  
  !Last row and column of E = 1
  E( 1:Ntot  ,    Ntot ) = ONE
  E(   Ntot  ,  1:Ntot ) = ONE
  E(   Ntot  ,    Ntot ) = ZERO  

  if ( DEBUG_PRINT .eqv. .TRUE. ) then
    call disp( 'Amat=' , pAmat , 'F7.4'  )
    call disp( 'Bmat=' , pBmat , 'F7.4'  )
    call disp( 'Cmat=' , pCmat , 'F7.4'  )
    call disp( 'Dmat=' , pDmat , 'F7.4'  )
    call disp( 'u='    , pu    , 'e12.6' )
  endif
  
  
  call cpu_time(t_fill_end)
  call cpu_time(t_solve_start)
  
  if (USE_LAPACK .eqv. .FALSE.) then
    charge = gsselm( E , Ni+No+1 )
  else
    allocate( ipiv(Ntot)  )
    call DGESV(  Ntot , 1 , E(1:Ntot,1:Ntot) , Ntot , ipiv , pu , Ntot , info )
    deallocate( ipiv )
    do n=1,Ntot
        charge(n) = pu( n , 1  )
    enddo
  endif
  
  inner_charge = sum( charge( 1:Ni ) )
  impedance(1) = sqrt(epsilon_r)/(C0*inner_charge)
  call cpu_time(t_solve_end)
  
  nullify( pAmat , pBmat , pCmat , pDmat , pu )
  deallocate( charge , inner_conductor , outer_conductor , E  )
  
  impedance(2) = real(Ni+No+1,wp)
  impedance(3) = t_fill_end-t_fill_start
  impedance(4) = t_solve_end-t_solve_start
  end function impedance


    !*  ************************************************************ */
    !*    Solving a system of linear equations by Gauss elimination
    !*      1st  matrix element -> (1,1)
    !*      Last matrix element -> (row,row)
    !*      plus an augmented column in the (row+1)th column
    !*      Input: a matrix of coefficients plus an augmented column - a(row,row+1)
    !*      Output: an array of solutions - Gsselm(row)                    */
    !*  ************************************************************ */
    function Gsselm(a,row)
    implicit none
    integer, intent(in)        :: row
    real(wp) , intent(inout)   ::  a(:,:)   !assume shape (:)
    real(wp) , dimension(row)  :: gsselm

    integer i,j,k
    integer, dimension(2)      :: shap
    real(wp) , allocatable     :: swap_ik(:)
    real(wp)                   :: tmp

    allocate (swap_ik(row+1))

    !Initialise
    swap_ik(:) = ZERO        ! Whole vector initialized to zero
    tmp = ZERO

    ! Check dimensions of input matrix
    shap = shape(a)
    if ( (shap(1) /= row) .OR.  (shap(2) /= row+1) ) then
        return
    end if


    !/*   Gaussian Elimination - Row Reduction of matrix  */
    do k=1, row-1                             ! total of row-1 operations

    !/*  Pivotal strategy - SWAP rows to make pivotal element a[k][k] have the
    !    greatest magnitude in its column. This prevents unnecessary division by
    !    a small number.                           */
        do i = k+1, row
            if ( ( abs(a(i,k)) - abs(a(k,k)) ) > TWO*epsilon(ZERO)  ) then
                do j = k, row+1                     !/* If pivotal element is not */
                    swap_ik(j) = a(k,j)              !/* the highest then  */
                    a(k,j) = a(i,j)                  !/* swap i'th and k'th rows */
                    a(i,j) = swap_ik(j)
                enddo !j-loop
            endif
        enddo !i-loop


!/*   If the Matrix is SINGULAR then EXIT program      */
        if ( abs(a(k,k)) < TWO*epsilon(ZERO) ) then
            print *,'After swapping rows to make the pivotal element become the'
            print *,'highest magnitude element in its column, its magnitude is'
            print *,'still extremely small.'
            print *,'Hence this is a SINGULAR MATRIX - no unique solution or '
            print *,'check that the input dimensions are correct.'
            return
       endif
   


!/*      Perform row-reduction with pivotal element a[k][k]     */
        do i = k+1, row
            do j = row+1, k, -1 !/* starting from end of column */
                a(i,j) = a(i,j) - a(k,j) / a(k,k) * a(i,k)
            enddo !/* end of j loop     */
        enddo !/* end of 2nd i loop */

    enddo !/* end of k loop     */
!  At this point, the bottom triangle is Zero


!/*   Back Substitution - Solutions of equations   */
    Gsselm(row) = a(row,row+1) / a(row,row)
    do k = row-1, 1, -1
        tmp = ZERO
        do j = k+1, row
            tmp = tmp + a(k,j)*Gsselm(j)
        enddo  !j-loop
        Gsselm(k) = ( a(k,row+1) - tmp ) / a(k,k)
    enddo !k-loop

    deallocate (swap_ik)
    return
    end function Gsselm

  
  pure function norm2squared( x , y )
    real(wp),dimension(2),intent(in) :: x,y
    real(wp)                         :: norm2squared
    norm2squared = ( x(1)-y(1) )*( x(1)-y(1) ) + ( x(2)-y(2) )*( x(2)-y(2) )
  end function norm2squared
    
  
  function trapezoidal( func , a , b, N )
    real(wp), intent(in)   :: a,b
    integer(ip),intent(in) :: N
    integer(ip)            :: k
    real(wp)               :: trapezoidal,h
    
    interface
      function func(x)
        use select_precision
        implicit none
        real(wp), intent(in) :: x
        real(wp)             :: func
      end function func
    end interface
    
    h = (b-a)/real(N-1_ip,wp)
    trapezoidal = ZERO
    do k=2_ip,N-1_ip
      trapezoidal = trapezoidal + TWO*func( real(k,wp)*h )
    enddo
    
    trapezoidal = trapezoidal + func( a )
    trapezoidal = trapezoidal + func( b )
    trapezoidal = HALF*h*trapezoidal
    return
    end function trapezoidal
end module mymodule
